/*
 * MapServer.cpp
 *
 *  Created on: Nov 6, 2010
 *      Author: bipins
 */
#include "MapServer.h"

MapServer::MapServer(ros::NodeHandle &n)	{
	sc = n.serviceClient<nav_msgs::GetMap>("/static_map");

}

/**
 * This method queries the map server and gets the map data.
 * This happens once only, at the when the node is constructed
 * It will return a MATRIX that contains 0,-1 and 100
 * #define WALL 100 indicates a wall
 * #define SPACE 0 indicates visitable space
 * #define NETHER -1 indicates space where the bot can never get, i.e., beyond the walls
 */
MATRIX MapServer::getMap()	{


	nav_msgs::GetMap srv;
	if (sc.call(srv))	{
		int cnt = 0;
		int width 		= srv.response.map.info.width;
		int height 		= srv.response.map.info.height;
		map_data_type &maptr = srv.response.map.data;

		map_data_type::iterator beg, end, itr;

		map.clear();
		for( itr = maptr.begin(), beg = maptr.begin(); itr != maptr.end(); itr++, cnt++ )
			if( cnt == width ){
				map.push_back(VECTOR(beg,itr));
				cnt = 0;
				beg = itr;
			}
		map.push_back(VECTOR(beg,itr));

		/** Sanity check **/
		assert( (int)map.size() == height );
		assert( (int)map[0].size() == width );
	}
	else{
		ROS_ERROR("Could not get map from map server. Is it not started, perhaps?");
	}

	return map;
}

